{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "d2623dee-249c-4ce9-ab49-b7148ad4b62c",
   "metadata": {},
   "source": [
    "<center><img src=\"./image/CLBLOGO.jpg\" alt=\"创乐博\" style=\"width: 300px;\"/></center>\n",
    "\n",
    "# 6.机器人超声波及红外避障\n",
    "\n",
    "@－－－－湖南创乐博智能科技有限公司－－－－<br>\n",
    "@  文件名：6.机器人超声波及红外避障.py <br>\n",
    "@  版本：V2.0 <br>\n",
    "@  author: zhulin<br>\n",
    "@  说明：机器人超声波及红外避障\n",
    "机器人基本运动！<br>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "8fca1605-775c-4ed4-9505-cc0cb291b1a0",
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "243 cm\n",
      "\n",
      "213 cm\n",
      "\n",
      "241 cm\n",
      "\n",
      "262 cm\n",
      "\n",
      "49 cm\n",
      "\n",
      "129 cm\n",
      "\n",
      "225 cm\n",
      "\n",
      "215 cm\n",
      "\n",
      "152 cm\n",
      "\n",
      "142 cm\n",
      "\n",
      "132 cm\n",
      "\n",
      "125 cm\n",
      "\n",
      "118 cm\n",
      "\n",
      "109 cm\n",
      "\n",
      "102 cm\n",
      "\n",
      "95 cm\n",
      "\n",
      "153 cm\n",
      "\n",
      "111 cm\n",
      "\n",
      "143 cm\n",
      "\n",
      "1201 cm\n",
      "\n",
      "114 cm\n",
      "\n",
      "115 cm\n",
      "\n",
      "120 cm\n",
      "\n",
      "131 cm\n",
      "\n",
      "255 cm\n",
      "\n",
      "178 cm\n",
      "\n",
      "388 cm\n",
      "\n",
      "1201 cm\n",
      "\n",
      "131 cm\n",
      "\n",
      "115 cm\n",
      "\n",
      "86 cm\n",
      "\n",
      "1201 cm\n",
      "\n",
      "1201 cm\n",
      "\n",
      "1201 cm\n",
      "\n",
      "247 cm\n",
      "\n",
      "200 cm\n",
      "\n",
      "70 cm\n",
      "\n",
      "46 cm\n",
      "\n",
      "131 cm\n",
      "\n",
      "109 cm\n",
      "\n",
      "91 cm\n",
      "\n",
      "78 cm\n",
      "\n",
      "68 cm\n",
      "\n",
      "66 cm\n",
      "\n",
      "53 cm\n",
      "\n",
      "50 cm\n",
      "\n",
      "52 cm\n",
      "\n",
      "56 cm\n",
      "\n",
      "49 cm\n",
      "\n",
      "50 cm\n",
      "\n",
      "53 cm\n",
      "\n",
      "52 cm\n",
      "\n",
      "55 cm\n",
      "\n",
      "55 cm\n",
      "\n",
      "50 cm\n",
      "\n",
      "50 cm\n",
      "\n",
      "50 cm\n",
      "\n",
      "50 cm\n",
      "\n",
      "50 cm\n",
      "\n",
      "49 cm\n",
      "\n",
      "50 cm\n",
      "\n",
      "50 cm\n",
      "\n",
      "49 cm\n",
      "\n",
      "50 cm\n",
      "\n",
      "50 cm\n",
      "\n",
      "54 cm\n",
      "\n"
     ]
    }
   ],
   "source": [
    "from LOBOROBOT import LOBOROBOT  # 载入机器人库\n",
    "import RPi.GPIO as GPIO\n",
    "import time\n",
    "\n",
    "BtnPin  = 19\n",
    "Gpin    = 5\n",
    "Rpin    = 6\n",
    "\n",
    "TRIG = 20\n",
    "ECHO = 21\n",
    "\n",
    "SensorRight = 16  # 右侧红外避障传感器\n",
    "SensorLeft  = 12  # 左侧红外避障传感器\n",
    "\n",
    "clbrobot = LOBOROBOT() # 实例化机器人对象\n",
    "        \n",
    "def keysacn():\n",
    "    val = GPIO.input(BtnPin)\n",
    "    while GPIO.input(BtnPin) == False:\n",
    "        val = GPIO.input(BtnPin)\n",
    "    while GPIO.input(BtnPin) == True:\n",
    "        time.sleep(0.01)\n",
    "        val = GPIO.input(BtnPin)\n",
    "        if val == True:\n",
    "            GPIO.output(Rpin,1)\n",
    "            while GPIO.input(BtnPin) == False:\n",
    "                GPIO.output(Rpin,0)\n",
    "        else:\n",
    "            GPIO.output(Rpin,0)\n",
    "            \n",
    "def setup():\n",
    "        GPIO.setwarnings(False)\n",
    "        GPIO.setmode(GPIO.BCM)\n",
    "        # 红外避障传感器接口\n",
    "        GPIO.setup(SensorRight,GPIO.IN)\n",
    "        GPIO.setup(SensorLeft,GPIO.IN)\n",
    "        # 超声波避障传感器接口\n",
    "        GPIO.setup(TRIG, GPIO.OUT)\n",
    "        GPIO.setup(ECHO, GPIO.IN)\n",
    "        \n",
    "        GPIO.setup(Gpin, GPIO.OUT)     # Set Green Led Pin mode to output\n",
    "        GPIO.setup(Rpin, GPIO.OUT)     # Set Red Led Pin mode to output\n",
    "        GPIO.setup(BtnPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)    # Set BtnPin's mode is input, and pull up to high level(3.3V)\n",
    "\n",
    "\n",
    "def distance():\n",
    "    GPIO.output(TRIG, 0)\n",
    "    time.sleep(0.000002)\n",
    "    \n",
    "    GPIO.output(TRIG, 1)\n",
    "    time.sleep(0.00001)\n",
    "    GPIO.output(TRIG, 0)\n",
    "    while GPIO.input(ECHO) == 0:\n",
    "        a = 0\n",
    "    time1 = time.time()\n",
    "    while GPIO.input(ECHO) == 1:\n",
    "        a = 1\n",
    "    time2 = time.time()\n",
    "    during = time2 - time1    \n",
    "    time.sleep(0.2)\n",
    "    return round(during * 340 / 2 * 100)\n",
    "\n",
    "def loop():\n",
    "        while True:\n",
    "            \n",
    "            SR_2 = GPIO.input(SensorRight)\n",
    "            SL_2 = GPIO.input(SensorLeft)           \n",
    "            if SL_2 == True and SR_2 ==False:\n",
    "                print(\"Left\")\n",
    "                clbrobot.turnLeft(50,0)\n",
    "            elif SL_2==False and SR_2 ==True:\n",
    "                print(\"Right\")\n",
    "                clbrobot.turnRight(50,0)\n",
    "            \n",
    "            dis = distance()\n",
    "            if (dis < 40) == True:\n",
    "                while (dis < 40) == True:\n",
    "                    clbrobot.t_down(50,0.5)\n",
    "                    clbrobot.turnRight(50,0.1)\n",
    "                    dis = distance()\n",
    "            else:\n",
    "                clbrobot.t_up(50,0)\n",
    "            \n",
    "            print(dis, 'cm')\n",
    "            print('')\n",
    "\n",
    "def destroy():\n",
    "        clbrobot.t_stop(.1)\n",
    "        GPIO.cleanup()\n",
    "\n",
    "if __name__ == \"__main__\":\n",
    "        setup()\n",
    "        clbrobot.t_stop(.1)\n",
    "        keysacn()\n",
    "        try:\n",
    "                loop()\n",
    "        except KeyboardInterrupt:\n",
    "                destroy()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
